
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_loiter.c
  * @author     baiyang
  * @date       2022-2-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_loiter.h"

#include <parameter/param.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#define LOITER_SPEED_DEFAULT                1250.0f // default loiter speed in cm/s
#define LOITER_SPEED_MIN                    20.0f   // minimum loiter speed in cm/s
#define LOITER_ACCEL_MAX_DEFAULT            500.0f  // default acceleration in loiter mode
#define LOITER_BRAKE_ACCEL_DEFAULT          250.0f  // minimum acceleration in loiter mode
#define LOITER_BRAKE_JERK_DEFAULT           500.0f  // maximum jerk in cm/s/s/s in loiter mode
#define LOITER_BRAKE_START_DELAY_DEFAULT    1.0f    // delay (in seconds) before loiter braking begins after sticks are released
#define LOITER_VEL_CORRECTION_MAX           200.0f  // max speed used to correct position errors in loiter
#define LOITER_POS_CORRECTION_MAX           200.0f  // max position error in loiter
#define LOITER_ACTIVE_TIMEOUT_MS            200     // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sanity_check_params(mc_loiter* loiter);
static void calc_desired_velocity(mc_loiter* loiter, float nav_dt, bool avoidance_on);
static void assign_param(mc_loiter* loiter);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   loiter  
  * @param[in]   ahrs  
  * @param[in]   pos_control  
  * @param[in]   attitude_control  
  * @param[out]  
  * @retval      
  * @note        
  */
void loiter_ctor(mc_loiter* loiter, ahrs_view* ahrs, Position_ctrl* pos_control, Attitude_ctrl* attitude_control)
{
    loiter->_ahrs             = ahrs;
    loiter->_pos_control      = pos_control;
    loiter->_attitude_control = attitude_control;

    assign_param(loiter);
}

/// init_target to a position in cm from ekf origin
void loiter_init_target2(mc_loiter* loiter, const Vector2f_t* position)
{
    sanity_check_params(loiter);

    // initialise position controller speed and acceleration
    posctrl_set_correction_speed_accel_xy(loiter->_pos_control, LOITER_VEL_CORRECTION_MAX, loiter->_accel_cmss);
    posctrl_set_pos_error_max_xy_cm(loiter->_pos_control, LOITER_POS_CORRECTION_MAX);

    // initialise position controller
    posctrl_init_xy_controller_stopping_point(loiter->_pos_control);

    // initialise desired acceleration and angles to zero to remain on station
    vec2_zero(&loiter->_predicted_accel);
    vec2_zero(&loiter->_desired_accel);
    vec2_zero(&loiter->_predicted_euler_angle);
    loiter->_brake_accel = 0.0f;

    // set target position
    posctrl_set_pos_target_xy_cm(loiter->_pos_control, position->x, position->y);
}

/// init_target to a position in cm from ekf origin
void loiter_init_target3(mc_loiter* loiter, const Vector3f_t* position)
{
    sanity_check_params(loiter);

    // initialise position controller speed and acceleration
    posctrl_set_correction_speed_accel_xy(loiter->_pos_control, LOITER_VEL_CORRECTION_MAX, loiter->_accel_cmss);
    posctrl_set_pos_error_max_xy_cm(loiter->_pos_control, LOITER_POS_CORRECTION_MAX);

    // initialise position controller
    posctrl_init_xy_controller_stopping_point(loiter->_pos_control);

    // initialise desired acceleration and angles to zero to remain on station
    vec2_zero(&loiter->_predicted_accel);
    vec2_zero(&loiter->_desired_accel);
    vec2_zero(&loiter->_predicted_euler_angle);
    loiter->_brake_accel = 0.0f;

    // set target position
    posctrl_set_pos_target_xy_cm(loiter->_pos_control, position->x, position->y);
}

/// initialize's position and feed-forward velocity from current pos and velocity
void loiter_init_target(mc_loiter* loiter)
{
    sanity_check_params(loiter);

    // initialise position controller speed and acceleration
    posctrl_set_correction_speed_accel_xy(loiter->_pos_control, LOITER_VEL_CORRECTION_MAX, loiter->_accel_cmss);
    posctrl_set_pos_error_max_xy_cm(loiter->_pos_control, LOITER_POS_CORRECTION_MAX);

    // initialise position controller
    posctrl_init_xy_controller(loiter->_pos_control);

    // initialise predicted acceleration and angles from the position controller
    loiter->_predicted_accel.x = posctrl_get_accel_target_cmss(loiter->_pos_control).x;
    loiter->_predicted_accel.y = posctrl_get_accel_target_cmss(loiter->_pos_control).y;
    loiter->_predicted_euler_angle.x = radians(posctrl_get_roll_cd(loiter->_pos_control)*0.01f);
    loiter->_predicted_euler_angle.y = radians(posctrl_get_pitch_cd(loiter->_pos_control)*0.01f);
    loiter->_brake_accel = 0.0f;
}

/// reduce response for landing
void loiter_soften_for_landing(mc_loiter* loiter)
{
    const Vector3f_t curr_pos = ahrs_get_position_neu_cm(loiter->_ahrs);

    // set target position to current position
    posctrl_set_pos_target_xy_cm(loiter->_pos_control, curr_pos.x, curr_pos.y);

    // also prevent I term build up in xy velocity controller. Note
    // that this flag is reset on each loop, in update_xy_controller()
    posctrl_set_externally_limited_xy(loiter->_pos_control);
}

/// set pilot desired acceleration in centi-degrees
//   dt should be the time (in seconds) since the last call to this function
void loiter_set_pilot_desired_acceleration(mc_loiter* loiter, float euler_roll_angle_cd, float euler_pitch_angle_cd)
{
    const float dt = posctrl_get_dt(loiter->_pos_control);
    // Convert from centidegrees on public interface to radians
    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);

    // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically
    const Vector3f_t desired_euler = {euler_roll_angle, euler_pitch_angle, loiter->_ahrs->yaw};
    const Vector3f_t desired_accel = posctrl_lean_angles_to_accel(&desired_euler);

    loiter->_desired_accel.x = desired_accel.x;
    loiter->_desired_accel.y = desired_accel.y;

    // difference between where we think we should be and where we want to be
    Vector2f_t angle_error = {math_wrap_PI(euler_roll_angle - loiter->_predicted_euler_angle.x), math_wrap_PI(euler_pitch_angle - loiter->_predicted_euler_angle.y)};

    // calculate the angular velocity that we would expect given our desired and predicted attitude
    attctrl_input_shaping_rate_predictor(loiter->_attitude_control, &angle_error, &loiter->_predicted_euler_rate, dt);

    // update our predicted attitude based on our predicted angular velocity
    vec2_add_mult(&loiter->_predicted_euler_angle, &loiter->_predicted_euler_rate, dt);

    // convert our predicted attitude to an acceleration vector assuming we are not accelerating vertically
    const Vector3f_t predicted_euler = {loiter->_predicted_euler_angle.x, loiter->_predicted_euler_angle.y, loiter->_ahrs->yaw};
    const Vector3f_t predicted_accel = posctrl_lean_angles_to_accel(&predicted_euler);

    loiter->_predicted_accel.x = predicted_accel.x;
    loiter->_predicted_accel.y = predicted_accel.y;
}

/// get vector to stopping point based on a horizontal position and velocity
void loiter_get_stopping_point_xy(mc_loiter* loiter, Vector2f_t* stopping_point)
{
    Vector2p stop;
    posctrl_get_stopping_point_xy_cm(loiter->_pos_control, &stop);

    stopping_point->x = stop.x;
    stopping_point->y = stop.y;
}

/// get maximum lean angle when using loiter
float loiter_get_angle_max_cd(mc_loiter* loiter)
{
    if (!math_flt_positive(loiter->_angle_max)) {
        return MIN(attctrl_lean_angle_max_cd(loiter->_attitude_control), posctrl_get_lean_angle_max_cd(loiter->_pos_control)) * (2.0f/3.0f);
    }
    return MIN(loiter->_angle_max*100.0f, posctrl_get_lean_angle_max_cd(loiter->_pos_control));
}

/// run the loiter controller
void loiter_update(mc_loiter* loiter, bool avoidance_on)
{
    calc_desired_velocity(loiter, posctrl_get_dt(loiter->_pos_control), avoidance_on);
    posctrl_update_xy_controller(loiter->_pos_control);
}

// sanity check parameters
static void sanity_check_params(mc_loiter* loiter)
{
    loiter->_speed_cms = MAX(loiter->_speed_cms, LOITER_SPEED_MIN);
    loiter->_accel_cmss = MIN(loiter->_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(attctrl_lean_angle_max_cd(loiter->_attitude_control) * 0.01f)));
}

/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
static void calc_desired_velocity(mc_loiter* loiter, float nav_dt, bool avoidance_on)
{
    float ekfGndSpdLimit = loiter->_pos_control->_psc_data.ahrsGndSpdLimit;
    float ahrsControlScaleXY = loiter->_pos_control->_psc_data.ahrsControlScaleXY;

    // calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
    // parameter and the value set by the EKF to observe optical flow limits
    float gnd_speed_limit_cms = MIN(loiter->_speed_cms, ekfGndSpdLimit*100.0f);
    gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);

    float pilot_acceleration_max = control_angle_to_accel(loiter_get_angle_max_cd(loiter)*0.01) * 100;

    // range check nav_dt
    if (nav_dt < 0) {
        return;
    }

    // get loiters desired velocity from the position controller where it is being stored.
    const Vector3f_t desired_vel_3d = posctrl_get_vel_desired_cms(loiter->_pos_control);
    Vector2f_t desired_vel = {desired_vel_3d.x,desired_vel_3d.y};

    // update the desired velocity using our predicted acceleration
    desired_vel.x += loiter->_predicted_accel.x * nav_dt;
    desired_vel.y += loiter->_predicted_accel.y * nav_dt;

    Vector2f_t loiter_accel_brake = VECTOR2F_DEFAULT_VALUE;
    float desired_speed = vec2_length(&desired_vel);
    if (!math_flt_zero(desired_speed)) {
        Vector2f_t desired_vel_norm;

        desired_vel_norm.x = desired_vel.x / desired_speed;
        desired_vel_norm.y = desired_vel.y / desired_speed;

        // TODO: consider using a velocity squared relationship like
        // pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2;
        // the drag characteristic of a multirotor should be examined to generate a curve
        // we could add a expo function here to fine tune it

        // calculate a drag acceleration based on the desired speed.
        float drag_decel = pilot_acceleration_max*desired_speed/gnd_speed_limit_cms;

        // calculate a braking acceleration if sticks are at zero
        float loiter_brake_accel = 0.0f;
        if (vec2_is_zero(&loiter->_desired_accel)) {
            if ((time_millis()-loiter->_brake_timer) > loiter->_brake_delay * 1000.0f) {
                float brake_gain = posctrl_get_vel_xy_pid(loiter->_pos_control)->_kp * 0.5f;
                loiter_brake_accel = math_constrain_float(control_sqrt_controller(desired_speed, brake_gain, loiter->_brake_jerk_max_cmsss, nav_dt), 0.0f, loiter->_brake_accel_cmss);
            }
        } else {
            loiter_brake_accel = 0.0f;
            loiter->_brake_timer = time_millis();
        }
        loiter->_brake_accel += math_constrain_float(loiter_brake_accel-loiter->_brake_accel, -loiter->_brake_jerk_max_cmsss*nav_dt, loiter->_brake_jerk_max_cmsss*nav_dt);

        loiter_accel_brake.x = desired_vel_norm.x * loiter->_brake_accel;
        loiter_accel_brake.y = desired_vel_norm.y * loiter->_brake_accel;

        // update the desired velocity using the drag and braking accelerations
        desired_speed = MAX(desired_speed-(drag_decel+loiter->_brake_accel)*nav_dt,0.0f);

        desired_vel.x = desired_vel_norm.x * desired_speed;
        desired_vel.y = desired_vel_norm.y * desired_speed;
    }

    // add braking to the desired acceleration
    loiter->_desired_accel.x -= loiter_accel_brake.x;
    loiter->_desired_accel.y -= loiter_accel_brake.y;

    // Apply EKF limit to desired velocity -  this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
    float horizSpdDem = vec2_length(&desired_vel);
    if (horizSpdDem > gnd_speed_limit_cms) {
        desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
        desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
    }

    // get loiters desired velocity from the position controller where it is being stored.
    Vector2p target_pos = {posctrl_get_pos_target_cm(loiter->_pos_control).x, posctrl_get_pos_target_cm(loiter->_pos_control).y};

    // update the target position using our predicted velocity
    target_pos.x += desired_vel.x * nav_dt;
    target_pos.y += desired_vel.y * nav_dt;

    // send adjusted feed forward acceleration and velocity back to the Position Controller
    posctrl_set_pos_vel_accel_xy(loiter->_pos_control, &target_pos, &desired_vel, &loiter->_desired_accel);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void assign_param(mc_loiter* loiter)
{
    param_link_variable(PARAM_ID(LOIT, LOIT_ANG_MAX), &loiter->_angle_max);
    param_link_variable(PARAM_ID(LOIT, LOIT_SPEED), &loiter->_speed_cms);
    param_link_variable(PARAM_ID(LOIT, LOIT_ACC_MAX), &loiter->_accel_cmss);
    param_link_variable(PARAM_ID(LOIT, LOIT_BRK_ACCEL), &loiter->_brake_accel_cmss);
    param_link_variable(PARAM_ID(LOIT, LOIT_BRK_JERK), &loiter->_brake_jerk_max_cmsss);
    param_link_variable(PARAM_ID(LOIT, LOIT_BRK_DELAY), &loiter->_brake_delay);
}

/*------------------------------------test------------------------------------*/


